#  res_sampler v 0.1
#  Copyright (C) 2010 John Donovan - mersey.viking@gmail.com

#  Licensed under the Apache License, Version 2.0 (the "License");
#  you may not use this file except in compliance with the License.
#  You may obtain a copy of the License at

#  http://www.apache.org/licenses/LICENSE-2.0

#  Unless required by applicable law or agreed to in writing, software
#  distributed under the License is distributed on an "AS IS" BASIS,
#  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
#  See the License for the specific language governing permissions and
#  limitations under the License.

import numpy as np
import time

_max = 0.0
_min = 0.0
contactThreshold = 9.0
amplitudeThreshold = 0.01
bufSize = 30
buf = np.zeros(bufSize)
i = 0

def signalStream():
    with open('signal.txt', 'r') as f:
        while 1:
            yield float(f.readline())
            #time.sleep(0.01) # Simulate real data rate.

def waiting(iter):
    global contactThreshold

    while 1:
        value = iter.next()

        if value < contactThreshold:
            yield 'filtering'
       
def filtering(iter):
    global _min
    global _max
    global amplitudeThreshold
    global buf
    global i
    
    while 1:
        buf = np.roll(buf, 1)
        buf[0] = iter.next()
        i = i + 1
        # Wait for the buffer to fill.
        if i >= bufSize:
            _max = np.amax(buf)
            _min = np.amin(buf)
            if _max >= contactThreshold:
                yield 'waiting'
            elif _max - _min < amplitudeThreshold:
                yield 'storing'

def onFilterExit():
    global buf
    global i
    print "onFilterExit()"
    buf = buf * 0.0
    i = 0

def storing():
    global _min
    global _max
    while 1:
        print _max - _min, (_max + _min) / 2.0        yield 'waiting_for_threshold'

def waitForThreshold(iter):
    global contactThreshold
    while 1:
        value = iter.next()
        if value >= contactThreshold:
            yield 'waiting'

def scheduler(state_dict, state):
    try:
        while 1:
            old_state = state

            # Optional OnEntry function.
            try:
                state_dict[state][1]()
            except TypeError:
                pass
                
            print "State: ", state
            state = state_dict[state][0].next()

            # Optional OnExit function.
            try:
                state_dict[old_state][2]()
            except TypeError:
                pass
    except ValueError:
        print "No more data."

if __name__ == "__main__":
    stream = signalStream()

    state_dict = {'waiting':                [waiting(stream), None, None],
                  'filtering':              [filtering(stream), None, onFilterExit],
                  'storing':                [storing(), None, None],
                  'waiting_for_threshold':  [waitForThreshold(stream), None, None]
                 }

    scheduler(state_dict, 'waiting')

